﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2TimeOfImpact.h
// Revision   : r90
// Change-Date: 2010-04-10
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2TimeOfImpact.cpp
// Revision   : r114
// Change-Date: 2010-06-19
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>
    /// Input parameters for TimeOfImpact
    /// </summary>
    public struct TOIInput
    {
        public DistanceProxy proxyA;
        public DistanceProxy proxyB;
        public Sweep sweepA;
        public Sweep sweepB;
        public float tMax;  // defines sweep interval [0, tMax]
    }

    /// <summary>
    /// Output parameters for TimeOfImpact
    /// </summary>
    public struct TOIOutput
    {
        public enum TOIOutputState
        {
            e_unknown,
            e_failed,
            e_overlapped,
            e_touching,
            e_separated,
        }

        public TOIOutputState state;
        public float t;
    }

    internal struct SeparationFunction
    {
        internal enum Type
        {
            e_points,
            e_faceA,
            e_faceB
        };

#if !ZUNE && !WINDOWS_PHONE && !SILVERLIGHT
        internal unsafe float Initialize(SimplexCache* cache,
                                         DistanceProxy proxyA, Sweep sweepA,
                                         DistanceProxy proxyB, Sweep sweepB, float t1_)
        {
            m_proxyA = proxyA;
            m_proxyB = proxyB;
            int count = cache->count;
            System.Diagnostics.Debug.Assert(0 < count && count < 3);

            m_sweepA = sweepA;
            m_sweepB = sweepB;

            Transform xfA, xfB;
            m_sweepA.GetTransform(out xfA, t1_);
            m_sweepB.GetTransform(out xfB, t1_);

            if (count == 1)
            {
                FaceType = Type.e_points;
                Vector2 localPointA = m_proxyA.GetVertex(cache->indexA[0]);
                Vector2 localPointB = m_proxyB.GetVertex(cache->indexB[0]);
#if !OPTIMIZE
                Vector2 pointA = Math.Mul(xfA, localPointA);
                Vector2 pointB = Math.Mul(xfB, localPointB);
#else
                Vector2 pointA;
                Vector2 t1; t1.X = xfA.R.col1.X * localPointA.X + xfA.R.col2.X * localPointA.Y; t1.Y = xfA.R.col1.Y * localPointA.X + xfA.R.col2.Y * localPointA.Y;
                pointA.X = xfA.position.X + t1.X;
                pointA.Y = xfA.position.Y + t1.Y;

                Vector2 pointB;
                Vector2 t2; t2.X = xfB.R.col1.X * localPointB.X + xfB.R.col2.X * localPointB.Y; t2.Y = xfB.R.col1.Y * localPointB.X + xfB.R.col2.Y * localPointB.Y;
                pointB.X = xfB.position.X + t2.X;
                pointB.Y = xfB.position.Y + t2.Y;
#endif
                m_axis.X = pointB.X - pointA.X; m_axis.Y = pointB.Y - pointA.Y;
                m_axis.Normalize(); ////////TODO: overload + return Length()
                return m_axis.Length();
            }
            else if (cache->indexA[0] == cache->indexA[1])
            {
                // Two points on B and one on A 
                FaceType = Type.e_faceB;

                Vector2 localPointB1 = proxyB.GetVertex(cache->indexB[0]);
                Vector2 localPointB2 = proxyB.GetVertex(cache->indexB[1]);

                m_axis = Vector2Extensions.Cross(localPointB2 - localPointB1, 1.0f);
                m_axis.Normalize();
                Vector2 normal = Math.Mul(xfB.R, m_axis);

                LocalPoint = 0.5f * (localPointB1 + localPointB2);
#if !OPTIMIZE
                Vector2 pointB = Math.Mul(xfB, LocalPoint);
#else
                Vector2 pointB;
                Vector2 t; t.X = xfB.R.col1.X * LocalPoint.X + xfB.R.col2.X * LocalPoint.Y; t.Y = xfB.R.col1.Y * LocalPoint.X + xfB.R.col2.Y * LocalPoint.Y;
                pointB.X = xfB.position.X + t.X;
                pointB.Y = xfB.position.Y + t.Y;
#endif

                Vector2 localPointA = proxyA.GetVertex(cache->indexA[0]);
#if !OPTIMIZE
                Vector2 pointA = Math.Mul(xfA, localPointA);
#else
                Vector2 pointA;
                t.X = xfA.R.col1.X * localPointA.X + xfA.R.col2.X * localPointA.Y; t.Y = xfA.R.col1.Y * localPointA.X + xfA.R.col2.Y * localPointA.Y;
                pointA.X = xfA.position.X + t.X;
                pointA.Y = xfA.position.Y + t.Y;
#endif

                Vector2 t2; t2.X = pointA.X - pointB.X; t2.Y = pointA.Y - pointB.Y;
                float s = t2.X * normal.X + t2.Y * normal.Y; // Vector2.Dot(ref t2, ref normal, out s);
                if (s < 0.0f)
                {
                    m_axis = -m_axis;
                    s = -s;
                }
                return s;
            }
            else
            {
                // Two points on A and one or two points on B.
                FaceType = Type.e_faceA;
                Vector2 localPointA1 = m_proxyA.GetVertex(cache->indexA[0]);
                Vector2 localPointA2 = m_proxyA.GetVertex(cache->indexA[1]);

                m_axis = Vector2Extensions.Cross(localPointA2 - localPointA1, 1.0f);
                m_axis.Normalize();
                Vector2 normal = Math.Mul(xfA.R, m_axis);

                LocalPoint = 0.5f * (localPointA1 + localPointA2);
#if !OPTIMIZE
                Vector2 pointA = Math.Mul(xfA, LocalPoint);

                Vector2 localPointB = m_proxyB.GetVertex(cache->indexB[0]);
                Vector2 pointB = Math.Mul(xfB, localPointB);
#else
                Vector2 pointA;
                Vector2 t; t.X = xfA.R.col1.X * LocalPoint.X + xfA.R.col2.X * LocalPoint.Y; t.Y = xfA.R.col1.Y * LocalPoint.X + xfA.R.col2.Y * LocalPoint.Y;
                pointA.X = xfA.position.X + t.X;
                pointA.Y = xfA.position.Y + t.Y;

                Vector2 localPointB = m_proxyB.GetVertex(cache->indexB[0]);

                Vector2 pointB;
                t.X = xfB.R.col1.X * localPointB.X + xfB.R.col2.X * localPointB.Y; t.Y = xfB.R.col1.Y * localPointB.X + xfB.R.col2.Y * localPointB.Y;
                pointB.X = xfB.position.X + t.X;
                pointB.Y = xfB.position.Y + t.Y;
#endif

                Vector2 t2; t2.X = pointB.X - pointA.X; t2.Y = pointB.Y - pointA.Y;
                float s = t2.X * normal.X + t2.Y * normal.Y; // Vector2.Dot(t2, normal);
                if (s < 0.0f)
                {
                    m_axis = -m_axis;
                    s = -s;
                }
                return s;
            }
        }
#else
        internal float Initialize(ref SimplexCache cache,
                                  DistanceProxy proxyA, Sweep sweepA,
                                  DistanceProxy proxyB, Sweep sweepB)
        {
            m_proxyA = proxyA;
            m_proxyB = proxyB;
            int count = cache.count;
            System.Diagnostics.Debug.Assert(0 < count && count < 3);

            m_sweepA = sweepA;
            m_sweepB = sweepB;

            Transform xfA, xfB;
            m_sweepA.GetTransform(out xfA, 0.0f);
            m_sweepB.GetTransform(out xfB, 0.0f);

            if (count == 1)
            {
                FaceType = Type.e_points;
                Vector2 localPointA = m_proxyA.GetVertex(cache.indexA[0]);
                Vector2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
#if !OPTIMIZE
                Vector2 pointA = Math.Mul(xfA, localPointA);
                Vector2 pointB = Math.Mul(xfB, localPointB);
#else
                Vector2 pointA;
                Vector2 t1; t1.X = xfA.R.col1.X * localPointA.X + xfA.R.col2.X * localPointA.Y; t1.Y = xfA.R.col1.Y * localPointA.X + xfA.R.col2.Y * localPointA.Y;
                pointA.X = xfA.position.X + t1.X;
                pointA.Y = xfA.position.Y + t1.Y;

                Vector2 pointB;
                Vector2 t2; t2.X = xfB.R.col1.X * localPointB.X + xfB.R.col2.X * localPointB.Y; t2.Y = xfB.R.col1.Y * localPointB.X + xfB.R.col2.Y * localPointB.Y;
                pointB.X = xfB.position.X + t2.X;
                pointB.Y = xfB.position.Y + t2.Y;
#endif
                m_axis.X = pointB.X - pointA.X; m_axis.Y = pointB.Y - pointA.Y;
                m_axis.Normalize(); ////////TODO: overload + return Length()
                return m_axis.Length();
            }
            else if (cache.indexA[0] == cache.indexA[1])
            {
                // Two points on B and one on A 
                FaceType = Type.e_faceB;

                Vector2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
                Vector2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);

                m_axis = Vector2Extensions.Cross(localPointB2 - localPointB1, 1.0f);
                m_axis.Normalize();
                Vector2 normal = Math.Mul(xfB.R, m_axis);

                LocalPoint = 0.5f * (localPointB1 + localPointB2);
#if !OPTIMIZE
                Vector2 pointB = Math.Mul(xfB, LocalPoint);
#else
                Vector2 pointB;
                Vector2 t; t.X = xfB.R.col1.X * LocalPoint.X + xfB.R.col2.X * LocalPoint.Y; t.Y = xfB.R.col1.Y * LocalPoint.X + xfB.R.col2.Y * LocalPoint.Y;
                pointB.X = xfB.position.X + t.X;
                pointB.Y = xfB.position.Y + t.Y;
#endif

                Vector2 localPointA = proxyA.GetVertex(cache.indexA[0]);
#if !OPTIMIZE
                Vector2 pointA = Math.Mul(xfA, localPointA);
#else
                Vector2 pointA;
                t.X = xfA.R.col1.X * localPointA.X + xfA.R.col2.X * localPointA.Y; t.Y = xfA.R.col1.Y * localPointA.X + xfA.R.col2.Y * localPointA.Y;
                pointA.X = xfA.position.X + t.X;
                pointA.Y = xfA.position.Y + t.Y;
#endif

                Vector2 t2; t2.X = pointA.X - pointB.X; t2.Y = pointA.Y - pointB.Y;
                float s = t2.X * normal.X + t2.Y * normal.Y; // Vector2.Dot(ref t2, ref normal, out s);
                if (s < 0.0f)
                {
                    m_axis = -m_axis;
                    s = -s;
                }
                return s;
            }
            else
            {
                // Two points on A and one or two points on B.
                FaceType = Type.e_faceA;
                Vector2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
                Vector2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);

                m_axis = Vector2Extensions.Cross(localPointA2 - localPointA1, 1.0f);
                m_axis.Normalize();
                Vector2 normal = Math.Mul(xfA.R, m_axis);

                LocalPoint = 0.5f * (localPointA1 + localPointA2);
#if !OPTIMIZE
                Vector2 pointA = Math.Mul(xfA, LocalPoint);

                Vector2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
                Vector2 pointB = Math.Mul(xfB, localPointB);
#else
                Vector2 pointA;
                Vector2 t; t.X = xfA.R.col1.X * LocalPoint.X + xfA.R.col2.X * LocalPoint.Y; t.Y = xfA.R.col1.Y * LocalPoint.X + xfA.R.col2.Y * LocalPoint.Y;
                pointA.X = xfA.position.X + t.X;
                pointA.Y = xfA.position.Y + t.Y;

                Vector2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);

                Vector2 pointB;
                t.X = xfB.R.col1.X * localPointB.X + xfB.R.col2.X * localPointB.Y; t.Y = xfB.R.col1.Y * localPointB.X + xfB.R.col2.Y * localPointB.Y;
                pointB.X = xfB.position.X + t.X;
                pointB.Y = xfB.position.Y + t.Y;
#endif

                Vector2 t2; t2.X = pointB.X - pointA.X; t2.Y = pointB.Y - pointA.Y;
                float s = t2.X * normal.X + t2.Y * normal.Y; // Vector2.Dot(t2, normal);
                if (s < 0.0f)
                {
                    m_axis = -m_axis;
                    s = -s;
                }
                return s;
            }
        }

#endif

        internal float FindMinSeparation(out Int32 indexA, out Int32 indexB, float t)
        {
            Transform xfA, xfB;
            m_sweepA.GetTransform(out xfA, t);
            m_sweepB.GetTransform(out xfB, t);

            switch (FaceType)
            {
                case Type.e_points:
                    {
#if !OPTIMIZE
                        Vector2 axisA = Math.MulT(xfA.R, m_axis);
                        Vector2 axisB = Math.MulT(xfB.R, -m_axis);

                        indexA = m_proxyA.GetSupport(ref axisA);
                        indexB = m_proxyB.GetSupport(ref axisB);

                        Vector2 localPointA = m_proxyA.GetVertex(indexA);
                        Vector2 localPointB = m_proxyB.GetVertex(indexB);

                        Vector2 pointA = Math.Mul(xfA, localPointA);
                        Vector2 pointB = Math.Mul(xfB, localPointB);
#else
                        Vector2 axisA = Vector2.Zero;
                        axisA.X = m_axis.X * xfA.R.col1.X + m_axis.Y * xfA.R.col1.Y; // Vector2.Dot(ref m_axis, ref xfA.R.col1, out axisA.X);
                        axisA.Y = m_axis.X * xfA.R.col2.X + m_axis.Y * xfA.R.col2.Y; // Vector2.Dot(ref m_axis, ref xfA.R.col2, out axisA.Y);

                        Vector2 axisB = Vector2.Zero;
                        Vector2 nAxis = -m_axis;
                        axisB.X = nAxis.X * xfB.R.col1.X + nAxis.Y * xfB.R.col1.Y; // Vector2.Dot(ref nAxis, ref xfB.R.col1, out axisB.X);
                        axisB.Y = nAxis.X * xfB.R.col2.X + nAxis.Y * xfB.R.col2.Y; // Vector2.Dot(ref nAxis, ref xfB.R.col2, out axisB.Y);

                        indexA = m_proxyA.GetSupport(ref axisA);
                        indexB = m_proxyB.GetSupport(ref axisB);

                        Vector2 localPointA = m_proxyA.GetVertex(indexA);
                        Vector2 localPointB = m_proxyB.GetVertex(indexB);

                        Vector2 pointA;
                        Vector2 tp; tp.X = xfA.R.col1.X * localPointA.X + xfA.R.col2.X * localPointA.Y; tp.Y = xfA.R.col1.Y * localPointA.X + xfA.R.col2.Y * localPointA.Y;
                        pointA.X = xfA.position.X + tp.X;
                        pointA.Y = xfA.position.Y + tp.Y;

                        Vector2 pointB;
                        tp.X = xfB.R.col1.X * localPointB.X + xfB.R.col2.X * localPointB.Y; tp.Y = xfB.R.col1.Y * localPointB.X + xfB.R.col2.Y * localPointB.Y;
                        pointB.X = xfB.position.X + tp.X;
                        pointB.Y = xfB.position.Y + tp.Y;
#endif
                        Vector2 tv1; tv1.X = pointB.X - pointA.X; tv1.Y = pointB.Y - pointA.Y;
                        float separation = tv1.X * m_axis.X + tv1.Y * m_axis.Y; // Vector2.Dot(tv1, m_axis);
                        return separation;
                    }
                case Type.e_faceA:
                    {
#if !OPTIMIZE
                        Vector2 normal = Math.Mul(xfA.R, m_axis);
                        Vector2 pointA = Math.Mul(xfA, LocalPoint);

                        Vector2 axisB = Math.MulT(xfB.R, -normal);

                        indexA = -1;
                        indexB = m_proxyB.GetSupport(ref axisB);

                        Vector2 localPointB = m_proxyB.GetVertex(indexB);
                        Vector2 pointB = Math.Mul(xfB, localPointB);

                        float separation = Vector2.Dot(pointB - pointA, normal);
#else
                        Vector2 normal, pointA, axisB = Vector2.Zero, localPointB, pointB, temp;

                        temp.X = xfA.R.col1.X * m_axis.X + xfA.R.col2.X * m_axis.Y;
                        temp.Y = xfA.R.col1.Y * m_axis.X + xfA.R.col2.Y * m_axis.Y;
                        normal.X = temp.X; normal.Y = temp.Y;

                        temp.X = xfA.R.col1.X * LocalPoint.X + xfA.R.col2.X * LocalPoint.Y; temp.Y = xfA.R.col1.Y * LocalPoint.X + xfA.R.col2.Y * LocalPoint.Y;
                        pointA.X = xfA.position.X + temp.X;
                        pointA.Y = xfA.position.Y + temp.Y;

                        Vector2 nNormal; nNormal.X = -normal.X; nNormal.Y = -normal.Y;
                        axisB.X = nNormal.X * xfB.R.col1.X + nNormal.Y * xfB.R.col1.Y; // Vector2.Dot(ref nNormal, ref xfB.R.col1, out axisB.X);
                        axisB.Y = nNormal.X * xfB.R.col2.X + nNormal.Y * xfB.R.col2.Y; // Vector2.Dot(ref nNormal, ref xfB.R.col2, out axisB.Y);

                        indexA = -1;
                        indexB = m_proxyB.GetSupport(ref axisB);

                        localPointB = m_proxyB.GetVertex(ref indexB);
                        temp.X = xfB.R.col1.X * localPointB.X + xfB.R.col2.X * localPointB.Y; temp.Y = xfB.R.col1.Y * localPointB.X + xfB.R.col2.Y * localPointB.Y;
                        pointB.X = xfB.position.X + temp.X;
                        pointB.Y = xfB.position.Y + temp.Y;

                        Vector2 tv1; tv1.X = pointB.X - pointA.X; tv1.Y = pointB.Y - pointA.Y;
                        float separation = tv1.X * normal.X + tv1.Y * normal.Y; // Vector2.Dot(tv1, normal);
#endif

                        return separation;
                    }
                case Type.e_faceB:
                    {
#if !OPTIMIZE
                        Vector2 normal = Math.Mul(xfB.R, m_axis);
                        Vector2 pointB = Math.Mul(xfB, LocalPoint);

                        Vector2 axisA = Math.MulT(xfA.R, -normal);

                        indexB = -1;
                        indexA = m_proxyA.GetSupport(ref axisA);

                        Vector2 localPointA = m_proxyA.GetVertex(indexA);
                        Vector2 pointA = Math.Mul(xfA, localPointA);

                        float separation = Vector2.Dot(pointA - pointB, normal);
#else
                        Vector2 normal, pointA, axisA = Vector2.Zero, localPointA, pointB, temp;

                        normal.X = xfB.R.col1.X * m_axis.X + xfB.R.col2.X * m_axis.Y;
                        normal.Y = xfB.R.col1.Y * m_axis.X + xfB.R.col2.Y * m_axis.Y;

                        temp.X = xfB.R.col1.X * LocalPoint.X + xfB.R.col2.X * LocalPoint.Y; temp.Y = xfB.R.col1.Y * LocalPoint.X + xfB.R.col2.Y * LocalPoint.Y;
                        pointB.X = xfB.position.X + temp.X;
                        pointB.Y = xfB.position.Y + temp.Y;

                        Vector2 mNormal; mNormal.X = -normal.X; mNormal.Y = -normal.Y;
                        axisA.X = mNormal.X * xfA.R.col1.X + mNormal.Y * xfA.R.col1.Y; // Vector2.Dot(ref mNormal, ref xfA.R.col1, out axisA.X);
                        axisA.Y = mNormal.X * xfA.R.col2.X + mNormal.Y * xfA.R.col2.Y; // Vector2.Dot(ref mNormal, ref xfA.R.col2, out axisA.Y);

                        indexB = -1;
                        indexA = m_proxyA.GetSupport(ref axisA);

                        localPointA = m_proxyA.GetVertex(indexA);
                        temp.X = xfA.R.col1.X * localPointA.X + xfA.R.col2.X * localPointA.Y; temp.Y = xfA.R.col1.Y * localPointA.X + xfA.R.col2.Y * localPointA.Y;
                        pointA.X = xfA.position.X + temp.X;
                        pointA.Y = xfA.position.Y + temp.Y;

                        Vector2 tv1; tv1.X = pointA.X - pointB.X; tv1.Y = pointA.Y - pointB.Y;
                        float separation = tv1.X * normal.X + tv1.Y * normal.Y; // Vector2.Dot(tv1, normal);
#endif

                        return separation;
                    }
                default:
                    {
                        System.Diagnostics.Debug.Assert(false);
                        indexA = -1;
                        indexB = -1;
                        return 0.0f;
                    }
            }
        }

        internal float Evaluate(Int32 indexA, Int32 indexB, float t)
        {
            Transform xfA, xfB;
            m_sweepA.GetTransform(out xfA, t);
            m_sweepB.GetTransform(out xfB, t);

            switch (FaceType)
            {
                case Type.e_points:
                    {
#if !OPTIMIZE
                        Vector2 axisA = Math.MulT(xfA.R, m_axis);
                        Vector2 axisB = Math.MulT(xfB.R, -m_axis);

                        Vector2 localPointA = m_proxyA.GetVertex(indexA);
                        Vector2 localPointB = m_proxyB.GetVertex(indexB);

                        Vector2 pointA = Math.Mul(xfA, localPointA);
                        Vector2 pointB = Math.Mul(xfB, localPointB);

                        float separation = Vector2.Dot(pointB - pointA, m_axis);
#else
                        Vector2 axisA, axisB, pointA, pointB, temp, nAxis;

                        Vector2.Dot(ref m_axis, ref xfA.R.col1, out axisA.X);
                        Vector2.Dot(ref m_axis, ref xfA.R.col2, out axisA.Y);
                        
                        nAxis = -m_axis;
                        Vector2.Dot(ref nAxis, ref xfA.R.col1, out axisB.X);
                        Vector2.Dot(ref nAxis, ref xfA.R.col2, out axisB.Y);

                        Vector2 localPointA = m_proxyA.GetVertex(indexA);
                        Vector2 localPointB = m_proxyB.GetVertex(indexB);

                        temp.X = xfA.R.col1.X * localPointA.X + xfA.R.col2.X * localPointA.Y; temp.Y = xfA.R.col1.Y * localPointA.X + xfA.R.col2.Y * localPointA.Y;
                        pointA.X = xfA.position.X + temp.X;
                        pointA.Y = xfA.position.Y + temp.Y;
                        
                        temp.X = xfB.R.col1.X * localPointB.X + xfB.R.col2.X * localPointB.Y; temp.Y = xfB.R.col1.Y * localPointB.X + xfB.R.col2.Y * localPointB.Y;
                        pointB.X = xfB.position.X + temp.X;
                        pointB.Y = xfB.position.Y + temp.Y;

                        Vector2 tv1; tv1.X = pointB.X - pointA.X; tv1.Y = pointB.Y - pointA.Y;
                        float separation = tv1.X * m_axis.X + tv1.Y * m_axis.Y; // Vector2.Dot(tv1, m_axis);
#endif
                   
                        return separation;
                    }

                case Type.e_faceA:
                    {
#if !OPTIMIZE
                        Vector2 normal = Math.Mul(xfA.R, m_axis);
                        Vector2 pointA = Math.Mul(xfA, LocalPoint);

                        Vector2 axisB = Math.MulT(xfB.R, -normal);

                        Vector2 localPointB = m_proxyB.GetVertex(indexB);
                        Vector2 pointB = Math.Mul(xfB, localPointB);

                        float separation = Vector2.Dot(pointB - pointA, normal);
#else
                        Vector2 normal, pointA, axisB, localPointB, pointB, temp;

                        normal.X = xfA.R.col1.X * m_axis.X + xfA.R.col2.X * m_axis.Y;
                        normal.Y = xfA.R.col1.Y * m_axis.X + xfA.R.col2.Y * m_axis.Y;

                        temp.X = xfA.R.col1.X * LocalPoint.X + xfA.R.col2.X * LocalPoint.Y; temp.Y = xfA.R.col1.Y * LocalPoint.X + xfA.R.col2.Y * LocalPoint.Y;
                        pointA.X = xfA.position.X + temp.X;
                        pointA.Y = xfA.position.Y + temp.Y;

                        Vector2 nNormal; nNormal.X = -normal.X; nNormal.Y = -normal.Y;
                        axisB.X = nNormal.X * xfB.R.col1.X + nNormal.Y * xfB.R.col1.Y; // Vector2.Dot(ref nNormal, ref xfB.R.col1, out axisB.X);
                        axisB.Y = nNormal.X * xfB.R.col2.X + nNormal.Y * xfB.R.col2.Y; // Vector2.Dot(ref nNormal, ref xfB.R.col2, out axisB.Y);

                        localPointB = m_proxyB.GetVertex(indexB);

                        temp.X = xfB.R.col1.X * localPointB.X + xfB.R.col2.X * localPointB.Y; temp.Y = xfB.R.col1.Y * localPointB.X + xfB.R.col2.Y * localPointB.Y;
                        pointB.X = xfB.position.X + temp.X;
                        pointB.Y = xfB.position.Y + temp.Y;

                        Vector2 tv1; tv1.X = pointB.X - pointA.X; tv1.Y = pointB.Y - pointA.Y;
                        float separation = tv1.X * normal.X + tv1.Y * normal.Y; // Vector2.Dot(tv1, normal);
#endif

                        return separation;
                    }

                case Type.e_faceB:
                    {
#if !OPTIMIZE
                        Vector2 normal = Math.Mul(xfB.R, m_axis);
                        Vector2 pointB = Math.Mul(xfB, LocalPoint);

                        Vector2 axisA = Math.MulT(xfA.R, -normal);

                        Vector2 localPointA = m_proxyA.GetVertex(indexA);
                        Vector2 pointA = Math.Mul(xfA, localPointA);

                        float separation = Vector2.Dot(pointA - pointB, normal);
#else
                        Vector2 normal, pointB, axisA, localPointA, pointA, temp;

                        normal.X = xfB.R.col1.X * m_axis.X + xfB.R.col2.X * m_axis.Y;
                        normal.Y = xfB.R.col1.Y * m_axis.X + xfB.R.col2.Y * m_axis.Y;

                        temp.X = xfB.R.col1.X * LocalPoint.X + xfB.R.col2.X * LocalPoint.Y; temp.Y = xfB.R.col1.Y * LocalPoint.X + xfB.R.col2.Y * LocalPoint.Y;
                        pointB.X = xfB.position.X + temp.X;
                        pointB.Y = xfB.position.Y + temp.Y;

                        Vector2 nNormal; nNormal.X = -normal.X; nNormal.Y = -normal.Y;
                        axisA.X = nNormal.X * xfA.R.col1.X + nNormal.Y * xfA.R.col1.Y; // Vector2.Dot(ref nNormal, ref xfA.R.col1, out axisA.X);
                        axisA.Y = nNormal.X * xfA.R.col2.X + nNormal.Y * xfA.R.col2.Y; // Vector2.Dot(ref nNormal, ref xfA.R.col2, out axisA.Y);

                        localPointA = m_proxyA.GetVertex(indexA);

                        temp.X = xfA.R.col1.X * localPointA.X + xfA.R.col2.X * localPointA.Y; temp.Y = xfA.R.col1.Y * localPointA.X + xfA.R.col2.Y * localPointA.Y;
                        pointA.X = xfA.position.X + temp.X;
                        pointA.Y = xfA.position.Y + temp.Y;

                        Vector2 tv1; tv1.X = pointA.X - pointB.X; tv1.Y = pointA.Y - pointB.Y;
                        float separation = tv1.X * normal.X + tv1.Y * normal.Y; // Vector2.Dot(tv1, normal);
#endif

                        return separation;
                    }

                default:
                    System.Diagnostics.Debug.Assert(false);
                    return 0.0f;
            }
        }

        internal DistanceProxy m_proxyA;
        internal DistanceProxy m_proxyB;
        internal Sweep m_sweepA, m_sweepB;
        internal Type FaceType;
        internal Vector2 LocalPoint;
        internal Vector2 m_axis;
    } 
 
    public partial class Collision
    {
        public static int ToiCalls;
        public static int ToiIters;
        public static int MaxToiIters;
        public static int ToiRootIters;
        public static int MaxToiRootIters;

#if !ZUNE && !WINDOWS_PHONE && !SILVERLIGHT
        // CCD via the secant method. 
        /// <summary> 
        /// Compute the time when two shapes begin to touch or touch at a closer distance. 
        /// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance. 
        /// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be 
        /// smaller than sum of the shape radii. 
        /// Warning the sweeps must have the same time interval. 
        /// </summary> 
        /// <returns> 
        /// The fraction between [0,1] in which the shapes first touch. 
        /// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch. 
        /// </returns> 
        public unsafe static void TimeOfImpact(out TOIOutput output, TOIInput input)
        {
            ++ToiCalls;

            output = new TOIOutput();
            output.state = TOIOutput.TOIOutputState.e_unknown;
            output.t = input.tMax;

            DistanceProxy proxyA = input.proxyA;
            DistanceProxy proxyB = input.proxyB;

            Sweep sweepA = input.sweepA;
            Sweep sweepB = input.sweepB;

            // Large rotations can make the root finder fail, so we normalize the
            // sweep angles.
            sweepA.Normalize();
            sweepB.Normalize();

            float tMax = input.tMax;

            float totalRadius = proxyA.m_radius + proxyB.m_radius;
            float target = System.Math.Max(PhysicSettings.LinearSlop, totalRadius - 3.0f * PhysicSettings.LinearSlop);
            float tolerance = 0.25f * PhysicSettings.LinearSlop;
            System.Diagnostics.Debug.Assert(target > tolerance);

            float t1 = 0.0f;
            const int k_maxIterations = 20;       // TODO_ERIN Settings 
            int iter = 0;

            // Prepare input for distance query. 
            SimplexCache cache = new SimplexCache();
            cache.count = 0;
            DistanceInput distanceInput;
            distanceInput.proxyA = input.proxyA;
            distanceInput.proxyB = input.proxyB;
            distanceInput.useRadii = false;

            // The outer loop progressively attempts to compute new separating axes.
            // This loop terminates when an axis is repeated (no progress is made).
            for (; ;)
            {
                Transform xfA, xfB;
                sweepA.GetTransform(out xfA, t1);
                sweepB.GetTransform(out xfB, t1);

                // Get the distance between shapes. We can also use the results
                // to get a separating axis.
                distanceInput.transformA = xfA;
                distanceInput.transformB = xfB;
                DistanceOutput distanceOutput = new DistanceOutput();
                Collision.Distance(out distanceOutput, ref cache, ref distanceInput);

                // If the shapes are overlapped, we give up on continuous collision.
                if (distanceOutput.distance <= 0.0f)
                {
                    // Failure!
                    output.state = TOIOutput.TOIOutputState.e_overlapped;
                    output.t = 0.0f;
                    break;
                }

                if (distanceOutput.distance < target + tolerance)
                {
                    // Victory!
                    output.state = TOIOutput.TOIOutputState.e_touching;
                    output.t = t1;
                    break;
                }

                // Initialize the separating axis.
                SeparationFunction fcn = new SeparationFunction();
                fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);

#if _FALSE 
                                 // Dump the curve seen by the root finder 
                                 { 
                                         const int32 N = 100; 
                                         float dx = 1.0f / N; 
                                         float xs[N+1]; 
                                         float fs[N+1]; 
  
                                         float x = 0.0f; 
  
                                         for (int32 i = 0; i <= N; ++i) 
                                         { 
                                                 sweepA.GetTransform(&xfA, x); 
                                                 sweepB.GetTransform(&xfB, x); 
                                                 float f = fcn.Evaluate(xfA, xfB) - target; 
  
                                                 printf("%g %g\n", x, f); 
  
                                                 xs[i] = x; 
                                                 fs[i] = f; 
  
                                                 x += dx; 
                                         } 
                                 } 
#endif

                // Compute the TOI on the separating axis. We do this by successively
                // resolving the deepest point. This loop is bounded by the number of vertices.
                bool done = false;
                float t2 = tMax;
                int pushBackIter = 0;
                for (;;)
                {
                    // Find the deepest point at t2. Store the witness point indices.
                    Int32 indexA, indexB;
                    float s2 = fcn.FindMinSeparation(out indexA, out indexB, t2);

                    // Is the final configuration separated?
                    if (s2 > target + tolerance)
                    {
                        // Victory!
                        output.state = TOIOutput.TOIOutputState.e_separated;
                        output.t = tMax;
                        done = true;
                        break;
                    }

                    // Has the separation reached tolerance?
                    if (s2 > target - tolerance)
                    {
                        // Advance the sweeps
                        t1 = t2;

                        break;
                    }

                    // Compute the initial separation of the witness points.
                    float s1 = fcn.Evaluate(indexA, indexB, t1);

                    // Check for initial overlap. This might happen if the root finder
                    // runs out of iterations.
                    if (s1 < target - tolerance)
                    {
                        output.state = TOIOutput.TOIOutputState.e_failed;
                        output.t = t1;
                        done = true;
                        break;
                    }

                    // Check for touching
                    if (s1 <= target + tolerance)
                    {
                        // Victory! t1 should hold the TOI (could be 0.0).
                        output.state = TOIOutput.TOIOutputState.e_touching;
                        output.t = t1;
                        done = true;
                        break;
                    }

                    // Compute 1D root of: f(x) - target = 0
                    int rootIterCount = 0;
                    float a1 = t1, a2 = t2;
                    for (; ;)
                    {
                        // Use a mix of the secant rule and bisection. 
                        float t;
                        if ((rootIterCount & 1) == 1)
                        {
                            // Secant rule to improve convergence. 
                            t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
                        }
                        else
                        {
                            // Bisection to guarantee progress. 
                            t = 0.5f * (a1 + a2);
                        }

                        float s = fcn.Evaluate(indexA, indexB, t);

                        if (System.Math.Abs(s - target) < tolerance)
                        {
                            // t2 holds a tentative value for t1
                            t2 = t;
                            break;
                        }

                        // Ensure we continue to bracket the root. 
                        if (s > target)
                        {
                            a1 = t;
                            s1 = s;
                        }
                        else
                        {
                            a2 = t;
                            s2 = s;
                        }

                        ++rootIterCount;
                        ++ToiRootIters;

                        if (rootIterCount == 50)
                        {
                            break;
                        }
                    }

                    MaxToiRootIters = System.Math.Max(MaxToiRootIters, rootIterCount);

                    ++pushBackIter;

                    if (pushBackIter == PhysicSettings.MaxPolygonVertices)
                    {
                        break;
                    }
                }

                ++iter;
                ++ToiIters;

                if (done)
                {
                    break;
                }

                if (iter == k_maxIterations)
                {
                    // Root finder got stuck. Semi-victory.
                    output.state = TOIOutput.TOIOutputState.e_failed;
                    output.t = t1;
                    break;
                }
            }

            MaxToiIters = System.Math.Max(MaxToiIters, iter);
        } 
#else
        // CCD via the secant method. 
        /// <summary> 
        /// Compute the time when two shapes begin to touch or touch at a closer distance. 
        /// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance. 
        /// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be 
        /// smaller than sum of the shape radii. 
        /// Warning the sweeps must have the same time interval. 
        /// </summary> 
        /// <returns> 
        /// The fraction between [0,1] in which the shapes first touch. 
        /// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch. 
        /// </returns> 
        public static void TimeOfImpact(out TOIOutput output, TOIInput input)
        {
            ++ToiCalls;

            output = new TOIOutput();
            output.state = TOIOutput.TOIOutputState.e_unknown;
            output.t = input.tMax;

            DistanceProxy proxyA = input.proxyA;
            DistanceProxy proxyB = input.proxyB;

            Sweep sweepA = input.sweepA;
            Sweep sweepB = input.sweepB;
            float tMax = input.tMax;

            float totalRadius = proxyA.m_radius + proxyB.m_radius;
            float target = System.Math.Max(PhysicSettings.LinearSlop, totalRadius - 3.0f * PhysicSettings.LinearSlop);
            float tolerance = 0.25f * PhysicSettings.LinearSlop;
            System.Diagnostics.Debug.Assert(target > tolerance);

            float t1 = 0.0f;
            const int k_maxIterations = 1000;       // TODO_ERIN Settings 
            int iter = 0;

            // Prepare input for distance query. 
            SimplexCache cache = new SimplexCache();
            cache.count = 0;
            DistanceInput distanceInput;
            distanceInput.proxyA = input.proxyA;
            distanceInput.proxyB = input.proxyB;
            distanceInput.useRadii = false;

            // The outer loop progressively attempts to compute new separating axes.
            // This loop terminates when an axis is repeated (no progress is made).
            for (; ;)
            {
                Transform xfA, xfB;
                sweepA.GetTransform(out xfA, t1);
                sweepB.GetTransform(out xfB, t1);

                // Get the distance between shapes. We can also use the results
                // to get a separating axis.
                distanceInput.transformA = xfA;
                distanceInput.transformB = xfB;
                DistanceOutput distanceOutput = new DistanceOutput();
                Collision.Distance(out distanceOutput, ref cache, ref distanceInput);

                // If the shapes are overlapped, we give up on continuous collision.
                if (distanceOutput.distance <= 0.0f)
                {
                    // Failure!
                    output.state = TOIOutput.TOIOutputState.e_overlapped;
                    output.t = 0.0f;
                    break;
                }

                // Initialize the separating axis.
                SeparationFunction fcn = new SeparationFunction();
                fcn.Initialize(ref cache, proxyA, sweepA, proxyB, sweepB);

#if _FALSE 
                                 // Dump the curve seen by the root finder 
                                 { 
                                         const int32 N = 100; 
                                         float dx = 1.0f / N; 
                                         float xs[N+1]; 
                                         float fs[N+1]; 
  
                                         float x = 0.0f; 
  
                                         for (int32 i = 0; i <= N; ++i) 
                                         { 
                                                 sweepA.GetTransform(&xfA, x); 
                                                 sweepB.GetTransform(&xfB, x); 
                                                 float f = fcn.Evaluate(xfA, xfB) - target; 
  
                                                 printf("%g %g\n", x, f); 
  
                                                 xs[i] = x; 
                                                 fs[i] = f; 
  
                                                 x += dx; 
                                         } 
                                 } 
#endif

                // Compute the TOI on the separating axis. We do this by successively
                // resolving the deepest point. This loop is bounded by the number of vertices.
                bool done = false;
                float t2 = tMax;
                for (;;)
                {
                    // Find the deepest point at t2. Store the witness point indices.
                    Int32 indexA, indexB;
                    float s2 = fcn.FindMinSeparation(out indexA, out indexB, t2);

                    // Is the final configuration separated?
                    if (s2 > target + tolerance)
                    {
                        // Victory!
                        output.state = TOIOutput.TOIOutputState.e_separated;
                        output.t = tMax;
                        done = true;
                        break;
                    }

                    // Is the final configuration touching?
                    if (s2 > target - tolerance)
                    {
                        // Victory!
                        output.state = TOIOutput.TOIOutputState.e_touching;
                        output.t = t2;
                        done = true;
                        break;
                    }

                    // Compute the initial separation of the witness points.
                    float s1 = fcn.Evaluate(indexA, indexB, t1);

                    // Check for initial overlap. This might happen if the root finder
                    // runs out of iterations.
                    if (s1 < target - tolerance)
                    {
                        output.state = TOIOutput.TOIOutputState.e_failed;
                        output.t = t1;
                        done = true;
                        break;
                    }

                    // Check for touching
                    if (s1 <= target + tolerance)
                    {
                        // Victory! t1 should hold the TOI (could be 0.0).
                        output.state = TOIOutput.TOIOutputState.e_touching;
                        output.t = t1;
                        done = true;
                        break;
                    }

                    // Compute 1D root of: f(x) - target = 0
                    int rootIterCount = 0;
                    float a1 = t1, a2 = t2;
                    for (; ;)
                    {
                        // Use a mix of the secant rule and bisection. 
                        float t;
                        if ((rootIterCount & 1) == 1)
                        {
                            // Secant rule to improve convergence. 
                            t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
                        }
                        else
                        {
                            // Bisection to guarantee progress. 
                            t = 0.5f * (a1 + a2);
                        }

                        float s = fcn.Evaluate(indexA, indexB, t);

                        if (System.Math.Abs(s - target) < tolerance)
                        {
                            // t2 holds a tentative value for t1
                            t2 = t;
                            break;
                        }

                        // Ensure we continue to bracket the root. 
                        if (s > target)
                        {
                            a1 = t;
                            s1 = s;
                        }
                        else
                        {
                            a2 = t;
                            s2 = s;
                        }

                        ++rootIterCount;
                        ++ToiRootIters;

                        if (rootIterCount == 50)
                        {
                            break;
                        }
                    }

                    MaxToiRootIters = System.Math.Max(MaxToiRootIters, rootIterCount);
                }

                ++iter;
                ++ToiIters;

                if (done)
                {
                    break;
                }

                if (iter == k_maxIterations)
                {
                    // Root finder got stuck. Semi-victory.
                    output.state = TOIOutput.TOIOutputState.e_failed;
                    output.t = t1;
                    break;
                }
            }

            MaxToiIters = System.Math.Max(MaxToiIters, iter);
        } 
#endif
    }
}
